package com.wltt.product.frfront.executors;

import com.wltt.product.frfront.enums.WeldButtonIdEnum;
import com.wltt.product.frfront.holder.JavafxHolder;
import com.wltt.product.frfront.holder.RobotHolder;
import com.wltt.product.frfront.model.bo.FormulaDetailBO;
import com.wltt.product.frfront.model.bo.WeldPointBO;
import com.wltt.product.frfront.model.po.SwingTypePO;
import com.wltt.product.frfront.model.vo.WeldREQ;
import fairino.DescPose;
import fairino.ExaxisPos;
import fairino.Robot;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.List;

public class FastArcWeldExecutor implements Runnable {

    private static final Logger logger = LogManager.getLogger(FastArcWeldExecutor.class);

    /**
     * 机器人对象
     */
    private Robot robot;

    /**
     * 运动参数
     */
    private WeldREQ weldREQ;
    private WeldPointBO containStartSecurePoint;
    private WeldPointBO containEndSecurePoint;
    private WeldPointBO containArcStartPoint;
    private List<WeldPointBO> jointPosList;
    private FormulaDetailBO formulaDetailBO;
    private SwingTypePO swingTypePO;

    public FastArcWeldExecutor(Robot robot, WeldREQ weldREQ,
                               WeldPointBO containStartSecurePoint, WeldPointBO containEndSecurePoint, WeldPointBO containArcStartPoint,
                               List<WeldPointBO> jointPosList,
                               FormulaDetailBO formulaDetailBO,
                               SwingTypePO swingTypePO) {
        this.robot = robot;
        this.weldREQ = weldREQ;
        this.containStartSecurePoint = containStartSecurePoint;
        this.containEndSecurePoint = containEndSecurePoint;
        this.containArcStartPoint = containArcStartPoint;
        this.jointPosList = jointPosList;
        this.formulaDetailBO = formulaDetailBO;
        this.swingTypePO = swingTypePO;
    }

    @Override
    public void run() {
        if (!RobotHolder.setBusy()) {
            return;
        }
        if (RobotHolder.isStopWeld()) {
            return;
        }
        boolean weave = false;
        boolean ARC = false;
        try {
            JavafxHolder.setStartWeldDisable(WeldButtonIdEnum.FAST_ARC_WELD.getCode(), true);
            robot.Mode(0);
            SpeedBO speedBO = WeldTool.calculateSpeed(weldREQ.getWeldingSpeed());
            robot.SetSpeed(speedBO.getGlobalSpeedPercent());
            int vel = speedBO.getMoveSpeedPercent();

            // 运动到开始安全点
            DescPose currentDescPose =  new DescPose();
            robot.GetActualTCPPose(currentDescPose);
            if (new BigDecimal(currentDescPose.tran.x).setScale(3, RoundingMode.HALF_UP).doubleValue() != containStartSecurePoint.getDescPose().tran.x
                    || new BigDecimal(currentDescPose.tran.y).setScale(3, RoundingMode.HALF_UP).doubleValue() != containStartSecurePoint.getDescPose().tran.y
                    || new BigDecimal(currentDescPose.tran.z).setScale(3, RoundingMode.HALF_UP).doubleValue() != containStartSecurePoint.getDescPose().tran.z
                    || new BigDecimal(currentDescPose.rpy.rx).setScale(3, RoundingMode.HALF_UP).doubleValue() != containStartSecurePoint.getDescPose().rpy.rx
                    || new BigDecimal(currentDescPose.rpy.ry).setScale(3, RoundingMode.HALF_UP).doubleValue() != containStartSecurePoint.getDescPose().rpy.ry
                    || new BigDecimal(currentDescPose.rpy.rz).setScale(3, RoundingMode.HALF_UP).doubleValue() != containStartSecurePoint.getDescPose().rpy.rz) {
                if (RobotHolder.isStopWeld()) {
                    return;
                }
                robot.MoveL(containStartSecurePoint.getJointPos(), containStartSecurePoint.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), 100, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);
            }

            // 运动到起弧点
//            robot.MoveJ(containArcStartPoint.getJointPos(), containArcStartPoint.getDescPose(),1, 0, 50, 100, 100, new ExaxisPos(), 0, 0, new DescPose());
            if (RobotHolder.isStopWeld()) {
                WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                return;
            }
            robot.MoveL(containArcStartPoint.getJointPos(), containArcStartPoint.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), 100, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);

            // 开始摆动
            if (weldREQ.getSwingEnabled()) {
                robot.WeaveOnlineSetPara(RobotHolder.getRobot().getRobotPO().getWeaveConfigNum(),Integer.parseInt(swingTypePO.getSwingTypeCode()), formulaDetailBO.getSwingFrequency(),
                        Integer.parseInt(formulaDetailBO.getWaitModeCode()), formulaDetailBO.getSwingRange(), formulaDetailBO.getSwingLeftRemainTime(), formulaDetailBO.getSwingRightRemainTime(), formulaDetailBO.getCircleSwingPullbackRatio(), Integer.parseInt(formulaDetailBO.getSwingLocationWaitCode()));
                if (RobotHolder.isStopWeld()) {
                    WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                    return;
                }
                robot.WeaveStart(RobotHolder.getRobot().getRobotPO().getWeaveConfigNum());
                weave = true;
            }
            // 开始焊接
            if (weldREQ.getWeldEnabled()) {
                robot.WeldingSetCurrent(0, weldREQ.getWeldingCurrent(), 0, 0);
                robot.WeldingSetVoltage(0, weldREQ.getWeldingVoltage(), 1, 0);
                if (RobotHolder.isStopWeld()) {
                    WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                    return;
                }
                robot.ARCStart(0, RobotHolder.getRobot().getRobotPO().getArcConfigNum(), 10000);
                ARC = true;
            }

            // 焊接路径
            for (int i = 0; i < jointPosList.size(); i = i + 2) {
                WeldPointBO weldPointBO_p = jointPosList.get(i);
                WeldPointBO weldPointBO_t = jointPosList.get(i + 1);
                if (RobotHolder.isStopWeld()) {
                    WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                    return;
                }
                robot.MoveC(weldPointBO_p.getJointPos(), weldPointBO_p.getDescPose(), RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), vel, 100, new ExaxisPos(), 0, new DescPose(),
                        weldPointBO_t.getJointPos(), weldPointBO_t.getDescPose(), RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), vel, 100, new ExaxisPos(), 0, new DescPose(), 100, 20);
                if (RobotHolder.isStopWeld()) {
                    WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                    return;
                }
            }

            // 停止焊接
            if (weldREQ.getWeldEnabled()) {
                robot.ARCEnd(0, RobotHolder.getRobot().getRobotPO().getArcConfigNum(), 10000);
                ARC = false;
            }

            // 停止摆动
            if (weldREQ.getWeldEnabled()) {
                robot.WeaveEnd(RobotHolder.getRobot().getRobotPO().getWeaveConfigNum());
                weave = false;
            }

            // 运动到安全点
            if (RobotHolder.isStopWeld()) {
                WeldTool.stopWeld(robot, weldREQ, weave, ARC);
                return;
            }
            robot.MoveL(containEndSecurePoint.getJointPos(), containEndSecurePoint.getDescPose(),RobotHolder.getRobot().getRobotPO().getToolCoordinateNum(), RobotHolder.getRobot().getRobotPO().getWorkpieceCoordinateNum(), vel, 100, 100, -1, new ExaxisPos(), 0, 0, new DescPose(), 0, 100);

        } finally {
            JavafxHolder.setStartWeldDisable(WeldButtonIdEnum.FAST_ARC_WELD.getCode(), false);
            RobotHolder.setNoBusy();
            robot.Mode(1);
        }
    }
}
